#include "LeadLag.h"

LeadLag_T* LeadLag_t;

//************************************
// Method:    	CALCU_SAMA					//< SAMA运算函数
// Programmer:  duan
// Time:		2019/06/24
// Returns:   	bool						//< 成功：true；失败：false
// Parameter: 	Model_Base_t * arg			//< 模块参数结构体指针
// Parameter: 	float dt					//< 运算周期
// Parameter: 	bool * DI					//< 数字量输入值数组指针
// Parameter: 	bool * DO					//< 数字量输出值数组指针
// Parameter: 	bool * AI					//< 模拟量输入值数组指针
// Parameter: 	bool * AO					//< 模拟量输出值数组指针
//************************************
bool CALCU_SAMA(LeadLag)(Model_Base_t* arg, float dt, bool* DI, bool* DO, float* AI, float* AO)
{

	/****************变量定义区开始**************/

	float a;
	/*<<<<<定义输入变量>>>>>*/
	float PV_Value;
	float T_Lead;						//< 超前时间常数
	float T_Lag;						//< 滞后时间常数

	/*<<<<<定义输出变量>>>>>*/
	float Output_Value;

	/*<<<<<定义中间变量>>>>>*/
	float* Mid_Value;

	/****************变量定义区结束**************/

	NAME(LeadLag)* Obj = (NAME(LeadLag)*)arg;

	/****************变量赋值**************/
	/*<<<<<输入变量赋值>>>>>*/
	PV_Value = AI[0];
	T_Lead = AI[1];
	T_Lag = AI[2];

	/*<<<<<中间变量赋值>>>>>*/
	Mid_Value = &(Obj->Sta.Mid_Value);

	/**************变量赋值结束************/

	/********************************************/
	/**************算法功能实现开始**************/
	/********************************************/
	
	////> ShiKe20200924 以下为原来的逻辑 师兄只实现了大于零 注释掉重新写了逻辑
	//if (T_Lead < 0 || T_Lag < 0||T_Lead==0||T_Lag==0)				////< duan 0701 目前只允许时间常数大于等于零 ->ShiKe20200924 其实师兄只实现了大于零
	//	return false;
	//if (T_Lag > 0)
	//{
	//	a = (float)exp(-dt / T_Lag);
	//	Mid_Value[0] = a * Mid_Value[0] + (a - 1)*(T_Lead / T_Lag - 1)*PV_Value;		//< X[k+1]=a*X[k]+(a-1)*(T_Lead / T_Lag -1)*U[k]
	//	Output_Value = Mid_Value[0] + (T_Lead / T_Lag)*PV_Value;						//< Y=X[k+1]+(T_Lead/T_Lag)*U[k]
	//}
	/*else
	{
		Output_Value = PV_Value;
		Mid_Value[0] = PV_Value;
	}*/
	////> ShiKe20200924 以上为原来的逻辑 师兄只实现了大于零 注释掉重新写了逻辑

	////> ShiKe20200924 以下为新的逻辑
	if (T_Lead < 0 || T_Lag < 0)				////<	ShiKe20200924 更改逻辑，现在可以允许时间常数等于零
		return false;							////<	ShiKe20200924 当超前和滞后其中一个小于零时

	if (T_Lag > 0)	////<	ShiKe20200924 滞后大于零 按照传递函数计算
	{
		a = (float)exp(-dt / T_Lag);
		Mid_Value[0] = a * Mid_Value[0] + (a - 1)*(T_Lead / T_Lag - 1)*PV_Value;		////< X[k+1]=a*X[k]+(a-1)*(T_Lead / T_Lag -1)*U[k]
		Output_Value = Mid_Value[0] + (T_Lead / T_Lag)*PV_Value;						////< Y=X[k+1]+(T_Lead/T_Lag)*U[k]
	}
	else ////<	ShiKe20200924 滞后等于零 如果超前也为零 那么就是1：1的比例，如果超前大于零 是纯微分无法实现，所以还是输出=输入
	{
		Output_Value = PV_Value;
		Mid_Value[0] = PV_Value;
	}
	
	/********************************************/
	/**************算法功能实现结束**************/
	/********************************************/
	
	/****************运算结果输出**************/
	AO[0] = Output_Value;
	
	/**************运算结果输出完毕************/
	return true;


}


//************************************
// Method:    	BAK_STA						//< 备份SAMA运算中间参数
// Programmer:  duan
// Time:		2019/06/24
// Returns:   	int							//< 成功：返回打包字节个数；失败：返回0
// Parameter: 	Model_Base_t * arg			//< 指定模块类型链表的指针
// Parameter: 	void ** Data				//< 存放数据包当前指针位置的地址
//************************************
int BAK_STA(LeadLag)(Model_Base_t* arg, void** Data)
{
	NAME(LeadLag)* Obj = (NAME(LeadLag)*)arg;
	BAK_STA_PRIVATE(Obj);
	return 0;
}



//************************************
// Method:    	ANA_STA						//< 解析备份的SAMA运算中间参数
// Programmer:  duan
// Time:		2019/06/24
// Returns:   	int							//< 成功：返回解析字节个数；失败：返回0
// Parameter: 	Model_Base_t * arg			//< 模块参数结构体指针
// Parameter: 	void ** Data				//< 存放数据包当前指针位置的地址
//************************************
int ANA_STA(LeadLag)(Model_Base_t* arg, void** Data)
{
	NAME(LeadLag)* Obj = (NAME(LeadLag)*)arg;
	ANA_STA_PRIVATE(Obj);
	return 0;
}


//************************************
// Method:    	INIT_STAPAR					//< 初始化模块运算中间参数
// Programmer:  duan
// Time:		2019/06/24
// Parameter: 	Model_Base_t* arg			//< 模块参数结构体指针
//************************************
void INIT_STAPAR(LeadLag)(Model_Base_t* arg)
{
	NAME(LeadLag)* name = (NAME(LeadLag)*)arg;
	/****************初始化中间变量结构体**************/
	memset(&(name->Sta), 0, sizeof(name->Sta));
	/**************初始化中间变量结构体结束************/

	/****************对中间变量结构体的相关参数进行赋值**************/

	/**************对中间变量结构体的相关参数进行赋值结束************/
}
